﻿import smbus2,time,math
import ctypes
from  base_mpu6050 import mpu_object



mpu = mpu_object()

# 初始化误差变量
AccErrorX = 0
AccErrorY = 0
GyroErrorX = 0
GyroErrorY = 0
GyroErrorZ = 0


def calculateError():
    global AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ
    c = 0
    # 读取加速度计数据200次
    while c < 200:
        AccX, AccY, AccZ = mpu.read_accel()
        # 计算并累加误差
        AccErrorX += math.atan(AccY / math.sqrt(AccX**2 + AccZ**2)) * 180 / math.pi
        AccErrorY += math.atan(-AccX / math.sqrt(AccY**2 + AccZ**2)) * 180 / math.pi
        c += 1
        time.sleep(0.1)

    # 求平均误差
    AccErrorX /= 200
    AccErrorY /= 200

    c = 0

    # 读取陀螺仪数据200次
    while c < 200:
        GyroX, GyroY, GyroZ = mpu.read_gyro()
        # 计算并累加误差
        GyroErrorX += GyroX
        GyroErrorY += GyroY
        GyroErrorZ += GyroZ
        c += 1
        time.sleep(0.1)

    # 求平均误差
    GyroErrorX /= 200
    GyroErrorY /= 200
    GyroErrorZ /= 200


calculateError()

print("误差读取完毕")

previousTime = time.time()


gyroAngleX = 0
gyroAngleY = 0
yaw = 0

roll = 0
pitch = 0
angle = 0



def calculate_angle():
    global gyroAngleX, gyroAngleY, yaw, roll, pitch, angle, previousTime

    # 获取当前时间
    currentTime = time.time()
    elapsedTime = currentTime - previousTime
    previousTime = currentTime

    # 读取加速度计数据
    AccX, AccY, AccZ = mpu.read_accel()
    # 计算加速度计的角度 (去除误差)
    denominator = math.sqrt(AccX**2 + AccZ**2)
    if denominator == 0:
        # 处理 AccX 和 AccZ 都为零的情况，给定一个默认值
        denominator = 1e-10  # 使用一个非常小的值来避免除以零
    accAngleX = (math.atan(AccY / denominator) * 180 / math.pi) - AccErrorX
    denominator = math.sqrt(AccY**2 + AccZ**2)
    if denominator == 0:
        # 处理 AccY 和 AccZ 都为零的情况，给定一个默认值
        denominator = 1e-10  # 使用一个非常小的值来避免除以零
    accAngleY = (math.atan(-AccX / denominator) * 180 / math.pi) - AccErrorY



    # 读取陀螺仪数据
    GyroX, GyroY, GyroZ = mpu.read_gyro()
    # 校正陀螺仪读数
    GyroX -= GyroErrorX
    GyroY -= GyroErrorY
    GyroZ -= GyroErrorZ

    # 将陀螺仪数据积分，得到角度
    gyroAngleX += GyroX * elapsedTime
    gyroAngleY += GyroY * elapsedTime
    yaw += GyroZ * elapsedTime

    # 融合加速度计和陀螺仪的数据
    roll = 0.96 * gyroAngleX + 0.04 * accAngleX
    pitch = 0.96 * gyroAngleY + 0.04 * accAngleY

    # 假设你需要的角度是roll
    angle = yaw

    return roll, pitch, yaw, angle

while True:

    calculate_angle()
    
    print(roll,pitch,yaw)








